ARAL 架构设计与模块调用流程

[TOC]

1. 架构总览

1.1 设计理念

ARAL（AUBO Robot Algorithm Library）采用分层模块化架构，遵循以下设计原则：

• 单一职责原则（SRP）: 每个模块负责且仅负责一个独立的功能领域
• 依赖倒置原则（DIP）: 高层模块不依赖底层实现，而是依赖抽象接口
• 开闭原则（OCP）: 对扩展开放，对修改封闭
• 工厂模式（Factory）: Scene 作为统一的工厂入口，管理所有模块的创建与生命周期

1.2 核心概念

1.2.1 架构层次关系

1.2.2 架构说明

层级结构:

• 层级 0 - Scene（场景层）:

  • 全局唯一的世界坐标系
  • 所有资源的工厂和生命周期管理器
  • 提供Utility工具集

• 层级 1 - Task（任务层）:

  • 一个Scene可以有多个Task
  • 负责场景级的同步控制（暂停、停止、恢复）
  • 协调多个Planner的执行

• 层级 2 - Planner（规划器层）:

  • 一个Task可以管理多个Planner
  • 每个Planner只规划一个运动轨迹
  • 每个Planner至多包含两个机器人（主从、联动、同步等）
  • Planner之间可以执行同步指令（同启同停）

• 层级 3 - Robot（机器人层）:

  • 每个机器人由Scene创建和管理
  • 每个机器人拥有专属的4个模块：
    • Model: 机器人物理模型和参数
    • Solver: 无状态的计算引擎
    • State: 实时状态管理器
    • Controller: 控制算法实现
  • 一个机器人可以同时被多个Planner调度

关键特性:

1. 多任务并行: 同一个Scene中可以运行多个Task，实现复杂的生产流程
2. 多规划器协同: 同一个Task中的多个Planner可以同步执行
3. 机器人复用: 同一个机器人可以被不同Task的Planner调度（时分复用）
4. 模块专属: 每个机器人的Model/Solver/State/Controller是专属的，保证状态隔离

2. 模块详解

2.1 Scene（场景）

职责: 全局场景管理器和工厂

核心功能:

• 维护唯一的世界坐标系
• 管理多个机器人实例
• 管理环境物体
• 创建和销毁所有子模块
• 级联生命周期管理

关键接口:

class Scene {
    // 机器人模型管理
    ModelHandle rlCreateRobotModel(const std::string& robot_name, const std::string& path);
    int rlDestroyRobotModel(const ModelHandle& handle);
    ModelPtr rlGetRobotModel(const ModelHandle& handle);

    // 获取工具模块
    UtilityPtr rlGetUtility();
    SolverPtr rlGetRobotSolver(const ModelHandle& handle);

    // 状态管理
    StateHandle rlCreateRobotState(const ModelHandle& handle, const std::string& state_name);
    StatePtr rlGetRobotState(const StateHandle& handle);

    // 控制与规划
    ControllerPtr rlCreateRobotController(const StateHandle& handle, const std::string& name);
    PlannerHandle rlCreateRobotPlanner(const std::vector<RobotRole>& robots, const std::string& name);
    PlannerPtr rlGetRobotPlanner(const PlannerHandle& handle);

    // 任务管理
    TaskPtr rlCreateRobotTask(const std::vector<PlannerHandle>& handles, const std::string& name);
};

使用场景:

// 创建全局唯一场景
auto scene = CreateARALScene("production_line");

// 场景内添加两个机器人
auto model_A_h = scene->rlCreateRobotModel("robot_A", "/path/A/");
auto model_B_h = scene->rlCreateRobotModel("robot_B", "/path/B/");

// 销毁机器人A（级联删除所有依赖模块）
scene->rlDestroyRobotModel(model_A_h);

2.2 Utility（工具集）

职责: 提供独立于机器人的通用算法和工具

核心功能:

• 日志管理（等级、回调）
• 错误码查询
• 版本信息
• 库配置
• 数学工具（向量运算、坐标变换）
• 滤波器（低通、卡尔曼、巴特沃斯、陷波）
• 几何工具（平面定义、距离计算）

关键接口:

class Utility {
    // 日志与配置
    int rlSetLogLevel(const LogLevel& level);
    int rlSetLogHandler(const LogHandler& logFunc);
    const std::string rlGetErrorDescription(const int errCode) const;

    // 数学工具
    RLPose utlChangeReferenceFrame(const RLPose& F_b_a, const RLPose& F_c_b) const;
    RLTwist utlCalDifferPose(const RLPose& F_a_b1, const RLPose& F_a_b2) const;

    // 滤波器
    int utlLowpassFilter(const double& sample_time, const double& cutoff_frequency, ...);
    int utlKalmanFilter(const double& Q, const double& R, ...);
    int utlButterFilter(const int& order, const double& fd, const double& fs, ...);

    // 环境物体管理
    int utlSetWorldObject(const std::string& object_id, const RLPose& pose, ...);
    int utlUnsetWorldObject(const std::string& object_id);
};

使用场景:

auto utility = scene->rlGetUtility();

// 设置日志
utility->rlSetLogLevel(LogLevel::INFO);

// 坐标变换
RLPose F_b_a = {0, 0, 0, 0, 0, 0};
RLPose F_c_b = {1, 0, 0, 0, 0, 0};
RLPose F_c_a = utility->utlChangeReferenceFrame(F_b_a, F_c_b);

// 添加环境物体
utility->utlSetWorldObject("table", pose, shapes, origins);

2.3 Model（机器人模型）

职责: 描述机器人的物理结构和参数（"机器人是什么"）

核心功能:

• 机器人结构描述（DOF、DH参数）
• 坐标系管理（工具、工件、传感器）
• 动力学参数（质量、惯量、摩擦）
• 运动限制（位置、速度、力矩）
• 功率限制
• 碰撞几何模型

关键接口:

class Model {
    // 基本信息
    unsigned int mdlGetRobotDOF() const;
    int mdlSetRobotDHParameterError(const std::vector<DHParameter>& error);

    // 坐标系管理
    int mdlSetToolPose(const RLPose& pose);
    int mdlGetToolPose(RLPose& pose);
    int mdlSetWorkpiecePose(const RLPose& pose);
    int mdlGetWorkpiecePose(RLPose& pose);
    int mdlEnableRemoteTool(const bool enable);

    // 动力学参数
    int mdlSetRobotLinkDynamicParameter(const std::vector<double>& real_para);
    std::vector<double> mdlGetRobotLinkDynamicParameter();
    int mdlSetLoadDynamicParameterInFlange(const RLInertia& inertia);
    int mdlSetJointFrictionParameter(const std::vector<FrictionParam>& friPara);

    // 运动限制
    int mdlSetJointPositionRange(const RLJntArray& upperLimit, const RLJntArray& lowerLimit);
    const RLJntArray mdlGetJointMaximumVelocity();
    int mdlSetJointStartStopTorque(const RLJntArray& startStopTorque);

    // 碰撞几何
    int mdlSetLinkCollisionGeometryModel(const std::string& link_name, ...);
    std::vector<OBBPairs> mdlGetLinkOBBModel(const std::string& link_name) const;
};

使用场景:

auto model = scene->rlGetRobotModel(model_handle);

// 配置工具和工件
RLPose tool_pose = {0, 0, 0.1, 0, 0, 0};  // 工具偏移100mm
model->mdlSetToolPose(tool_pose);

RLPose workpiece_pose = {0.5, 0, 0.5, 0, 0, 0};  // 工件位置
model->mdlSetWorkpiecePose(workpiece_pose);

// 设置负载参数
RLInertia load = {5.0, {0, 0, 0.05}, ...};  // 5kg负载
model->mdlSetLoadDynamicParameterInFlange(load);

// 设置运动限制
RLJntArray max_vel = {3.14, 3.14, 3.14, 3.14, 3.14, 3.14};
model->mdlSetJointPositionRange(upper, lower);

2.4 Solver（求解器）

职责: 提供无状态的计算引擎（"计算什么"）

核心功能:

• 运动学计算（正运动学、逆运动学、雅可比）
• 动力学计算（逆动力学、正动力学、重力补偿）
• 碰撞检测与距离计算
• 工作空间分析
• 标定算法（运动学、动力学）

关键接口:

class Solver {
    // 正运动学
    int kdCalForwardPosition(const RLJntArray& q_in, const RLPose& T_f_e, 
                            const bool& real_fk, RLPose& T_b_e) const;
    int kdCalForwardVelocity(const RLJntArray& q_in, const RLJntArray& dq_in, 
                            const RLPose& T_f_e, RLTwist& v_b_e) const;

    // 逆运动学
    int kdCalInversePosition(const IKConfigInfo& config, const RLPose& T_f_e, 
                            const bool& real_ik, RLJntArray& q_out) const;
    int kdCalAllInversePositions(const IKConfigInfo& config, const RLPose& T_f_e, 
                                const bool& real_ik, std::vector<RLJntArray>& q_out) const;
    int kdCalInverseVelocity(const RLJntArray& q_in, const RLTwist& v_in, 
                            const RLPose& T_f_e, RLJntArray& dq_out) const;

    // 动力学
    int kdCalInverseDynamics(const RLJntArray& joint_pos, const RLJntArray& joint_vel, 
                            const RLJntArray& joint_acc, const RLPose& T_f_e,
                            const std::vector<RLWrench>& ft_ext, RLJntArray& joint_torque) const;
    int kdCalGravityTorque(const RLJntArray& q, const RLInertia& payload, 
                          RLJntArray& robot_gravity, RLJntArray& payload_gravity) const;

    // 碰撞检测
    int kdCheckRobotCollision(const RLJntArray& joint, const bool& checkSelf, 
                             GeometryCollisionResult& result) const;
    int kdCalMinimumDistance(const RLJntArray& joint, const double& threshold, 
                            const bool& checkSelf, GeometryDistanceResult& result) const;

    // 标定
    int calibEndFramePosInFlange(const std::vector<RLPose>& F_b_f_calib_point, 
                                 Array3d& P_f_end, double& error);
    int calibToolDynamicParameter(const CalibTrajectoryFeature& traj_feature, 
                                  const DoubleVecVec& data_raw, ToolCalibResult& result);
};

使用场景:

auto solver = scene->rlGetRobotSolver(model_handle);

// 正运动学
RLJntArray q = {0, 0, 0, 0, 0, 0};
RLPose tcp_pose;
solver->kdCalForwardPosition(q, tool_pose, false, tcp_pose);

// 逆运动学
IKConfigInfo ik_config;
ik_config.target_pose = {0.5, 0, 0.5, 0, 0, 0};
ik_config.ref_joint = {0, 0, 0, 0, 0, 0};
RLJntArray q_sol;
int ret = solver->kdCalInversePosition(ik_config, tool_pose, false, q_sol);

// 碰撞检测
GeometryCollisionResult collision_result;
solver->kdCheckRobotCollision(q, true, collision_result);
if (collision_result.collision) {
    std::cout << "Collision detected!" << std::endl;
}

// 动力学
RLJntArray torque;
solver->kdCalInverseDynamics(q, qd, qdd, tool_pose, ft_ext, torque);

设计特点:

• ✅ 完全无状态: 所有函数都是 const 成员函数
• ✅ 纯计算: 只依赖输入参数，不保存任何中间状态
• ✅ 易于测试: 可以独立测试，不需要复杂的上下文
• ✅ 线程安全: 天然支持多线程并发调用

2.5 State（状态）

职责: 跟踪和管理机器人的实时状态（"机器人在哪"）

核心功能:

• 关节状态更新（位置、速度、加速度、力矩、温度）
• 传感器数据更新（末端力、底座力、关节力）
• 参考轨迹设置
• 外力估计与碰撞检测
• 速度限制管理
• 奇异性监测

关键接口:

class State {
    // 状态初始化
    int rsInitiateRobotState(const RLJntArray& q, const RLJntArray& qd, 
                            const RLJntArray& qdd, const ToolWorkpiece& t_w);

    // 关节状态更新（周期性调用）
    int rsUpdateJointState(const RLJntArray& q, const RLJntArray& qd, 
                          const RLJntArray& qdd, const RLJntArray& torque, 
                          const RLJntArray& temperature, const RLJntArray& friction, 
                          const bool& is_user_acc = false);

    // 传感器数据更新
    int rsUpdateFTSensorData(const FTSensorType& type, const double* data);

    // 参考轨迹设置
    int rsSetReferenceTrajectory(const bool& has_ref_traj, 
                                const TrajectoryPoint& point = TrajectoryPoint());

    // 速度限制
    int rsSetJointMaximumSpeed(const RLJntArray& speed);
    int rsSetEndMaximumSpeedInBase(const Array2d& speed);

    // 状态查询
    const RLPose rsGetEndPoseInBase() const;
    const RLWrench rsGetEndExternalWrenchInBase() const;
    const RLWrench rsGetEndExternalWrenchInEnd() const;
    const RLJntArray rsGetJointExternalTorque() const;
    const RLJntArray rsGetJointDynamicTorque() const;
    const SingularProperty rsGetSingularityProperty();
};

使用场景:

auto state = scene->rlGetRobotState(state_handle);

// 初始化状态
RLJntArray q0 = {0, 0, 0, 0, 0, 0};
RLJntArray qd0 = {0, 0, 0, 0, 0, 0};
RLJntArray qdd0 = {0, 0, 0, 0, 0, 0};
ToolWorkpiece tw;  // 工具和工件信息
state->rsInitiateRobotState(q0, qd0, qdd0, tw);

// 周期性更新（控制周期：5ms）
while (running) {
    // 从硬件读取关节状态
    RLJntArray q, qd, qdd, torque, temp, friction;
    robot_hardware->getJointState(q, qd, torque, temp);

    // 更新状态
    state->rsUpdateJointState(q, qd, qdd, torque, temp, friction);

    // 更新力传感器数据
    double ft_data[6];
    ft_sensor->read(ft_data);
    state->rsUpdateFTSensorData(FTSensorType::END_FT_SENSOR, ft_data);

    // 查询末端位姿
    RLPose tcp_pose = state->rsGetEndPoseInBase();

    // 查询外力
    RLWrench external_wrench = state->rsGetEndExternalWrenchInEnd();

    std::this_thread::sleep_for(std::chrono::milliseconds(5));
}

设计特点:

• 🔄 实时性: 支持高频率（200Hz）状态更新
• 📊 状态估计: 内部集成卡尔曼滤波进行状态估计
• 🛡️ 安全监测: 自动进行速度、奇异性监测

2.6 Controller（控制器）

职责: 实现各种控制算法（位置控制、力控制、阻抗控制等）

核心功能:

• 控制模式切换（位置、导纳、阻抗）
• 力控参数设置（刚度、阻尼、质量）
• 安全速度限制
• 奇异防护
• 振动抑制

关键接口:

class Controller {
    // 控制初始化
    int mcInitiateControlPara(const DescribeSpace& space, const RLPose& frame);
    int mcSetControlType(const ControlMode& type);

    // 力控参数
    int fcSetStiffness(const DescribeSpace& space, const double* stiffness);
    int fcSetDamp(const DescribeSpace& space, const double* damp);
    int fcSetMass(const DescribeSpace& space, const double* mass);
    int fcSetGoalForce(const DescribeSpace& space, const RLPose& frame, const RLWrench& force);

    // 安全限制
    int mcSetSafetyJointSpeed(const RLJntArray& speed);
    int mcSetSafetyTranslationSpeed(const double& vel);
    int mcSetSafetyRotationSpeed(const double& rot);

    // 振动抑制
    int mcSetVibSuppressParams(const int& level, const DoubleVec& omega, const DoubleVec& zeta);
};

使用场景:

auto controller = scene->rlCreateRobotController(state_handle, "force_ctrl");

// 配置力控模式
controller->mcSetControlType(ControlMode::ADMITTANCE);

// 设置笛卡尔刚度和阻尼
double stiffness[6] = {500, 500, 1000, 50, 50, 50};  // N/m, Nm/rad
double damp[6] = {50, 50, 100, 5, 5, 5};             // N*s/m, Nm*s/rad
controller->fcSetStiffness(DescribeSpace::CARTESIAN, stiffness);
controller->fcSetDamp(DescribeSpace::CARTESIAN, damp);

// 设置目标力
RLWrench target_force = {0, 0, -20, 0, 0, 0};  // Z方向20N压力
RLPose frame = {0, 0, 0, 0, 0, 0};  // 基坐标系
controller->fcSetGoalForce(DescribeSpace::CARTESIAN, frame, target_force);

2.7 Planner（规划器）

职责: 生成机器人的运动轨迹（"去哪里"）

核心功能:

• 路径添加（直线、圆弧、样条、关节）
• 轨迹规划（速度、加速度规划）
• 路径交融（blending）
• 避障规划
• 轨迹跟踪
• 速度调整

关键接口:

class Planner {
    // 规划器配置
    int tpSetPlannerCapacity(const PlannerCapacity& type, const unsigned int& size);
    unsigned int tpGetPlannerCapacity(const PlannerCapacity& type) const;
    int tpGetPlannerDepth(const PlannerQueue& queue) const;

    // 路径添加
    int tpAddPositionLine(const PathPoint& point, const PathProperty& path_property, 
                         const MoveProperty& move_property);
    int tpAddPoints(const std::vector<PathPoint>& points, const PathProperty& path_property, 
                   const MoveProperty& move_property, const ToolWorkpiece& tool_workpiece);
    int tpAddVelocityLine(const int& id, const DescribeSpace& space, 
                         const MoveProperty& move_property, const ToolWorkpiece& tool_workpiece);
    int tpSetEndPath();  // 终止交融

    // 轨迹跟踪
    int tpTargetMotionTracking(const PathPoint& point, const MoveProperty& move_property, 
                              const ToolWorkpiece& tool_workpiece, const unsigned int buffSize, 
                              const bool verbose = true);
    double tpTrajectoryTracking(const PathPoint& point, const MoveProperty& move_property, 
                               const ToolWorkpiece& tool_workpiece, const double& smooth_scale, 
                               const double& delay_sacle, const TrajectoryTrackingStatus& status);

    // 避障规划
    int tpGenerateCollisionFreePoints(const std::vector<PathPoint>& in_points, 
                                     const ObstacleAvoidanceParameters& obstacle_avoidance_params, 
                                     const ToolWorkpiece& tool_workpiece, 
                                     std::vector<PathPoint>& out_points);

    // 状态查询
    double tpGetMoveDuration(const double& pathId) const;
    double tpGetPlannerLeftMoveDuration() const;
    int tpGetPausedPoint(TrajectoryPoint& point, const Array2d& ik_eps);
    int tpGetPathPointAtGivenTime(const double& sample_period, const unsigned int& n, 
                                  std::vector<PathPoint>& points) const;

    // 恢复运动
    int tpSetResumePoint(const PathPoint& from, const PathProperty& path_property, 
                        const MoveProperty& move_property);
};

使用场景:

auto planner = scene->rlGetRobotPlanner(planner_handle);

// 配置规划器
planner->tpSetPlannerCapacity(PlannerCapacity::LookAhead, 10);

// 添加直线运动
PathPoint target1;
target1.describe_space = DescribeSpace::CARTESIAN;
target1.path_id = 1;
target1.pose = {0.5, 0, 0.5, 0, 0, 0};

PathProperty path_prop;
path_prop.describe_space = DescribeSpace::CARTESIAN;
path_prop.blend_radius = 0.01;  // 10mm交融半径

MoveProperty move_prop;
move_prop.max_vel = {0.5, 3.14};  // 线速度0.5m/s, 角速度3.14rad/s
move_prop.max_acc = {1.0, 6.28};

planner->tpAddPositionLine(target1, path_prop, move_prop);

// 添加圆弧运动
PathPoint via, target2;
via.pose = {0.6, 0.1, 0.5, 0, 0, 0};
target2.pose = {0.7, 0, 0.5, 0, 0, 0};
std::vector<PathPoint> arc_points = {target1, via, target2};
planner->tpAddPoints(arc_points, path_prop, move_prop, tool_workpiece);

// 结束路径交融
planner->tpSetEndPath();

2.8 Task（任务管理器）

职责: 协调多个规划器的执行（"怎么协调"）

核心功能:

• 任务状态管理（运行、暂停、停止）
• 多规划器同步控制
• 轨迹点采样
• 速度全局调整
• 异常处理

关键接口:

class Task {
    // 任务控制
    int tskSetTaskState(const TaskState& state);
    TaskState tskGetTaskState() const;
    int tskSetCycle(const double& period);

    // 同步控制
    int tskSyncMoveOn();   // 开启同步运动
    int tskWaitSyncMove(); // 等待同步运动

    // 速度控制
    int tskSetCartesianVelocityLimits(const Array2d& cartesianVelLimits, 
                                     const unsigned int buffSize);
    int tskSetVelocityScaleFactor(const double& velScal, const unsigned int buffSize);
    const Array2d tskGetCartesianVelocityLimits() const;
    double tskGetVelocityScaledFactor();

    // 轨迹采样（周期性调用）
    int tskUpdateCycle(const double& cycle, std::vector<TrajectoryPoint>& point, 
                      PlannerHandle& failed_planner);

    // 停止与恢复
    int tskStop(const StopType& type, const double& acc_ratio, const unsigned int buffSize);
    int tskResume();

    // 状态查询
    double tskGetLeftMoveDuration();
};

使用场景:

auto task = scene->rlCreateRobotTask({planner_A_h, planner_B_h}, "sync_task");

// 设置控制周期
task->tskSetCycle(0.005);  // 5ms

// 设置速度限制
Array2d vel_limits = {0.5, 3.14};  // 线速度、角速度
task->tskSetCartesianVelocityLimits(vel_limits, 10);

// 开启同步运动
task->tskWaitSyncMove();
planner_A->tpAddPositionLine(target_A, path_prop, move_prop);
planner_B->tpAddPositionLine(target_B, path_prop, move_prop);
task->tskSyncMoveOn();  // A和B同启同停

// 周期性采样轨迹点
std::vector<TrajectoryPoint> trajectory_points;
PlannerHandle failed_planner;
while (running) {
    int ret = task->tskUpdateCycle(0.005, trajectory_points, failed_planner);

    if (ret == 0) {
        // 正常输出轨迹点
        robot_A->moveToPoint(trajectory_points[0]);
        robot_B->moveToPoint(trajectory_points[1]);
    } else if (ret == 40002) {
        // 轨迹执行完成
        std::cout << "Motion completed!" << std::endl;
        break;
    } else if (ret < 0) {
        // 出错处理
        std::cerr << "Error: " << ret << " in planner " << failed_planner << std::endl;
        task->tskStop(StopType::EMERGENCY, 1.0, 0);
        break;
    }

    std::this_thread::sleep_for(std::chrono::milliseconds(5));
}

3. 模块依赖关系

3.1 管理层次关系（从架构图角度）

层级 0: Scene（场景）
        │
        ├─── Utility（工具集，独立）
        │
层级 1: └─── Task 1, Task 2, ... Task N（任务层）
                │
层级 2:         └─── Planner 1-1, 1-2, ... 1-N（规划器层）
                        │
层级 3:                 └─── Robot A, B, C, ...（机器人层）
                                │
                                └─── Model, Solver, State, Controller

说明：

• Scene 创建和管理所有 Task
• Task 协调管理多个 Planner（同步控制）
• Planner 调度 Robot（一个Planner最多包含2个Robot）
• Robot 拥有专属的4个模块：Model、Solver、State、Controller

3.2 创建依赖关系（从模块创建角度）

对于单个Robot内部的模块创建，存在以下依赖链：

Scene
  │
  ├─── Model（机器人模型）
  │      │
  │      ├─── Solver（基于Model创建，无状态计算引擎）
  │      │
  │      └─── State（基于Model创建，需要Model和Solver）
  │             │
  │             ├─── Controller（基于State创建）
  │             │
  │             └─── Planner（基于State创建，可包含多个Robot的State）
  │
  └─── Task（基于Planner创建，管理多个Planner）

创建顺序规则：

1. 先创建 Model（定义机器人"是什么"）
2. 再创建 State（需要依赖Model和Solver）
3. 然后创建 Planner（需要依赖State）
4. 最后创建 Task（需要依赖Planner）

3.3 生命周期级联关系

当销毁上层模块时，所有依赖它的下层模块也会被自动销毁：

3.3.1 Robot内部的级联删除

销毁 Model ──> 级联销毁 Solver
           ──> 级联销毁 State
                  ──> 级联销毁 Controller
                  ──> 级联销毁依赖该State的Planner（该Planner从Task中移除）

3.3.2 Task和Planner的关系

• 销毁Task：不会影响Planner，Planner仍然存在但不再被该Task管理
• 销毁Planner：不会影响Task，Task继续管理其他Planner
• 销毁Robot的Model：会导致依赖该Robot的Planner失效，但Task本身不会被销毁

示例1 - 销毁Robot导致Planner失效:

// 创建完整的依赖链
auto model_h = scene->rlCreateRobotModel("robot_A", path);
auto solver = scene->rlGetRobotSolver(model_h);
auto state_h = scene->rlCreateRobotState(model_h, "state");
auto planner_h = scene->rlCreateRobotPlanner({RobotRole{state_h, MASTER}}, "planner");
auto task = scene->rlCreateRobotTask({planner_h}, "task");

// 销毁 Model 会级联销毁所有依赖
scene->rlDestroyRobotModel(model_h);
// 此时 solver, state 失效，planner_h 对应的Planner失效
// task 本身依然存在，但其管理的planner已失效

示例2 - 多Robot场景的级联删除:

// 创建两个机器人和各自的规划器
auto model_A_h = scene->rlCreateRobotModel("robot_A", path_A);
auto model_B_h = scene->rlCreateRobotModel("robot_B", path_B);
auto state_A_h = scene->rlCreateRobotState(model_A_h, "state_A");
auto state_B_h = scene->rlCreateRobotState(model_B_h, "state_B");

auto planner_A_h = scene->rlCreateRobotPlanner({RobotRole{state_A_h, MASTER}}, "planner_A");
auto planner_B_h = scene->rlCreateRobotPlanner({RobotRole{state_B_h, MASTER}}, "planner_B");

auto task = scene->rlCreateRobotTask({planner_A_h, planner_B_h}, "sync_task");

// 销毁 Robot A
scene->rlDestroyRobotModel(model_A_h);
// planner_A 失效，但 task 和 planner_B 仍然有效
// task->tskUpdateCycle() 只会返回 Robot B 的轨迹点

4. 典型调用流程

4.1 单机器人运动控制流程

// 1. 创建场景
auto scene = CreateARALScene("cell_1");
auto utility = scene->rlGetUtility();
utility->rlSetLogLevel(LogLevel::INFO);

// 2. 创建机器人模型
auto model_h = scene->rlCreateRobotModel("robot_A", "/path/aubo_i5/");
auto model = scene->rlGetRobotModel(model_h);

// 3. 配置机器人参数
RLPose tool_pose = {0, 0, 0.1, 0, 0, 0};
model->mdlSetToolPose(tool_pose);

RLInertia load = {2.0, {0, 0, 0.03}, ...};
model->mdlSetLoadDynamicParameterInFlange(load);

// 4. 创建求解器和状态
auto solver = scene->rlGetRobotSolver(model_h);
auto state_h = scene->rlCreateRobotState(model_h, "real_state");
auto state = scene->rlGetRobotState(state_h);

// 5. 初始化状态
RLJntArray q0 = {0, 0, 0, 0, 0, 0};
ToolWorkpiece tw;
state->rsInitiateRobotState(q0, {0,0,0,0,0,0}, {0,0,0,0,0,0}, tw);

// 6. 创建规划器
auto planner_h = scene->rlCreateRobotPlanner({RobotRole{state_h, MASTER}}, "main_planner");
auto planner = scene->rlGetRobotPlanner(planner_h);

// 7. 添加运动
PathPoint target;
target.describe_space = DescribeSpace::CARTESIAN;
target.pose = {0.5, 0, 0.5, 0, 0, 0};

PathProperty path_prop;
path_prop.describe_space = DescribeSpace::CARTESIAN;

MoveProperty move_prop;
move_prop.max_vel = {0.5, 3.14};
move_prop.max_acc = {1.0, 6.28};

planner->tpAddPositionLine(target, path_prop, move_prop);

// 8. 创建任务
auto task = scene->rlCreateRobotTask({planner_h}, "main_task");
task->tskSetCycle(0.005);

// 9. 控制循环
while (true) {
    // 9.1 更新机器人状态
    RLJntArray q, qd, torque, temp, friction;
    hardware->getJointState(q, qd, torque, temp, friction);
    state->rsUpdateJointState(q, qd, {0,0,0,0,0,0}, torque, temp, friction);

    // 9.2 获取规划轨迹点
    std::vector<TrajectoryPoint> traj_points;
    PlannerHandle failed_h;
    int ret = task->tskUpdateCycle(0.005, traj_points, failed_h);

    if (ret == 0 && !traj_points.empty()) {
        // 9.3 下发轨迹点
        hardware->setJointCommand(traj_points[0].joint_pos, 
                                 traj_points[0].joint_vel,
                                 traj_points[0].joint_acc);
    } else if (ret == 40002) {
        std::cout << "Motion completed!" << std::endl;
        break;
    } else if (ret < 0) {
        std::cerr << "Planning error: " << ret << std::endl;
        task->tskStop(StopType::EMERGENCY, 1.0, 0);
        break;
    }

    std::this_thread::sleep_for(std::chrono::milliseconds(5));
}

4.2 多机器人协同运动流程（同步不联动）

参考 3.17.多机同步规划说明.md 中的场景三。

// 1. 创建场景
auto scene = CreateARALScene("dual_arm_cell");

// 2. 创建两个机器人
auto model_A_h = scene->rlCreateRobotModel("robot_A", "/path/A/");
auto model_B_h = scene->rlCreateRobotModel("robot_B", "/path/B/");

// 3. 创建状态
auto state_A_h = scene->rlCreateRobotState(model_A_h, "state_A");
auto state_B_h = scene->rlCreateRobotState(model_B_h, "state_B");

auto state_A = scene->rlGetRobotState(state_A_h);
auto state_B = scene->rlGetRobotState(state_B_h);

// 4. 初始化状态
RLJntArray q_A0 = {0, 0, 0, 0, 0, 0};
RLJntArray q_B0 = {0, 0, 0, 0, 0, 0};
ToolWorkpiece tw;

state_A->rsInitiateRobotState(q_A0, {0,0,0,0,0,0}, {0,0,0,0,0,0}, tw);
state_B->rsInitiateRobotState(q_B0, {0,0,0,0,0,0}, {0,0,0,0,0,0}, tw);

// 5. 为每个机器人创建独立规划器
auto planner_A_h = scene->rlCreateRobotPlanner(
    {RobotRole{state_A_h, PlannerRole::MASTER}}, "planner_A");
auto planner_B_h = scene->rlCreateRobotPlanner(
    {RobotRole{state_B_h, PlannerRole::MASTER}}, "planner_B");

auto planner_A = scene->rlGetRobotPlanner(planner_A_h);
auto planner_B = scene->rlGetRobotPlanner(planner_B_h);

// 6. 创建同步任务（关键：一个Task管理两个Planner）
auto task_sync = scene->rlCreateRobotTask({planner_A_h, planner_B_h}, "sync_task");
task_sync->tskSetCycle(0.005);

// 7. 添加同步运动
task_sync->tskWaitSyncMove();  // 开始同步运动

// 机器人A: 走一条直线
PathPoint target_A;
target_A.describe_space = DescribeSpace::CARTESIAN;
target_A.pose = {0.5, 0, 0.5, 0, 0, 0};
planner_A->tpAddPositionLine(target_A, path_prop, move_prop);

// 机器人B: 走一条直线和一条圆弧
PathPoint target_B1, via_B, target_B2;
target_B1.pose = {-0.5, 0, 0.5, 0, 0, 0};
via_B.pose = {-0.6, 0.1, 0.5, 0, 0, 0};
target_B2.pose = {-0.7, 0, 0.5, 0, 0, 0};

planner_B->tpAddPositionLine(target_B1, path_prop, move_prop);
std::vector<PathPoint> arc = {target_B1, via_B, target_B2};
planner_B->tpAddPoints(arc, path_prop, move_prop, tw);

task_sync->tskSyncMoveOn();  // 结束同步运动定义，A和B将同启同停

// 8. 控制循环
while (true) {
    // 8.1 更新两个机器人的状态
    RLJntArray q_A, qd_A, torque_A, temp_A, friction_A;
    RLJntArray q_B, qd_B, torque_B, temp_B, friction_B;

    hardware_A->getJointState(q_A, qd_A, torque_A, temp_A, friction_A);
    hardware_B->getJointState(q_B, qd_B, torque_B, temp_B, friction_B);

    state_A->rsUpdateJointState(q_A, qd_A, {0,0,0,0,0,0}, torque_A, temp_A, friction_A);
    state_B->rsUpdateJointState(q_B, qd_B, {0,0,0,0,0,0}, torque_B, temp_B, friction_B);

    // 8.2 获取同步轨迹点（关键：一次返回两个机器人的轨迹点）
    std::vector<TrajectoryPoint> traj_points;  // [point_A, point_B]
    PlannerHandle failed_h;
    int ret = task_sync->tskUpdateCycle(0.005, traj_points, failed_h);

    if (ret == 0 && traj_points.size() == 2) {
        // 8.3 同时下发两个机器人的轨迹点
        hardware_A->setJointCommand(traj_points[0].joint_pos, 
                                   traj_points[0].joint_vel,
                                   traj_points[0].joint_acc);
        hardware_B->setJointCommand(traj_points[1].joint_pos, 
                                   traj_points[1].joint_vel,
                                   traj_points[1].joint_acc);
    } else if (ret == 40002) {
        std::cout << "Synchronized motion completed!" << std::endl;
        break;
    } else if (ret < 0) {
        std::cerr << "Sync error: " << ret << " in planner " << failed_h << std::endl;
        task_sync->tskStop(StopType::EMERGENCY, 1.0, 0);
        break;
    }

    std::this_thread::sleep_for(std::chrono::milliseconds(5));
}

4.3 多机器人联动运动流程（联动不同步）

参考 3.17.多机同步规划说明.md 中的场景二。

// 场景：机器人A（旋转台）带动工件，机器人B的工具在工件上作业

// 1-4步同上（创建场景、模型、状态）

// 5. 创建联动规划器（关键：B是MASTER，A是REFERENCE或SLAVE）
bool A_independent = true;  // A是否独立规划

PlannerHandle planner_A_h, planner_B_h;
if (A_independent) {
    // 情况1：A独立规划
    planner_A_h = scene->rlCreateRobotPlanner(
        {RobotRole{state_A_h, PlannerRole::MASTER}}, "planner_A");
    planner_B_h = scene->rlCreateRobotPlanner(
        {RobotRole{state_A_h, PlannerRole::SLAVE},   // A作为从动
         RobotRole{state_B_h, PlannerRole::MASTER}}, "planner_B");
} else {
    // 情况2：A由外部控制
    planner_B_h = scene->rlCreateRobotPlanner(
        {RobotRole{state_A_h, PlannerRole::REFERENCE},  // A仅作参考
         RobotRole{state_B_h, PlannerRole::MASTER}}, "planner_B");
}

auto planner_B = scene->rlGetRobotPlanner(planner_B_h);

// 6. 添加运动（B在工件坐标系下规划）
// B的工具在工件坐标系下走正方形
PathPoint corner1, corner2, corner3, corner4;
corner1.describe_space = DescribeSpace::CARTESIAN;
corner1.pose = {0.1, 0.1, 0, 0, 0, 0};   // 工件坐标系
corner2.pose = {0.1, -0.1, 0, 0, 0, 0};
corner3.pose = {-0.1, -0.1, 0, 0, 0, 0};
corner4.pose = {-0.1, 0.1, 0, 0, 0, 0};

planner_B->tpAddPositionLine(corner1, path_prop, move_prop);
planner_B->tpAddPositionLine(corner2, path_prop, move_prop);
planner_B->tpAddPositionLine(corner3, path_prop, move_prop);
planner_B->tpAddPositionLine(corner4, path_prop, move_prop);
planner_B->tpAddPositionLine(corner1, path_prop, move_prop);

// 7. 创建任务
TaskPtr task_coord;
if (A_independent) {
    task_coord = scene->rlCreateRobotTask({planner_A_h, planner_B_h}, "coord_task");
} else {
    task_coord = scene->rlCreateRobotTask({planner_B_h}, "coord_task");
}

// 8. 控制循环
while (true) {
    // 8.1 更新状态
    if (A_independent) {
        // A自己规划，正常更新
        state_A->rsUpdateJointState(q_A, qd_A, qdd_A, torque_A, temp_A, friction_A);
    } else {
        // A由外部控制，必须更新A的状态供B参考
        state_A->rsUpdateJointState(q_A, qd_A, qdd_A, torque_A, temp_A, friction_A);
    }
    state_B->rsUpdateJointState(q_B, qd_B, qdd_B, torque_B, temp_B, friction_B);

    // 8.2 获取轨迹点
    std::vector<TrajectoryPoint> traj_points;
    PlannerHandle failed_h;
    int ret = task_coord->tskUpdateCycle(0.005, traj_points, failed_h);

    if (ret == 0) {
        if (A_independent && traj_points.size() == 2) {
            // A和B都有轨迹点
            hardware_A->setJointCommand(traj_points[0]);
            hardware_B->setJointCommand(traj_points[1]);
        } else if (!A_independent && traj_points.size() == 1) {
            // 只有B有轨迹点，A由外部控制
            hardware_B->setJointCommand(traj_points[0]);
        }
    } else if (ret == 40002) {
        std::cout << "Coordinated motion completed!" << std::endl;
        break;
    }

    std::this_thread::sleep_for(std::chrono::milliseconds(5));
}

4.4 力控制流程

// 1-5步同上（创建场景、模型、求解器、状态）

// 6. 创建控制器
auto controller = scene->rlCreateRobotController(state_h, "force_controller");

// 7. 配置力控参数
controller->mcInitiateControlPara(DescribeSpace::CARTESIAN, {0,0,0,0,0,0});
controller->mcSetControlType(ControlMode::ADMITTANCE);

// 设置刚度（笛卡尔空间）
double stiffness[6] = {500, 500, 1000, 50, 50, 50};  // N/m, Nm/rad
controller->fcSetStiffness(DescribeSpace::CARTESIAN, stiffness);

// 设置阻尼
double damp[6] = {50, 50, 100, 5, 5, 5};  // N*s/m, Nm*s/rad
controller->fcSetDamp(DescribeSpace::CARTESIAN, damp);

// 设置选择向量（只在Z方向力控）
int selection[6] = {0, 0, 1, 0, 0, 0};  // 只控制Z方向
controller->fcSetSelectionVector(DescribeSpace::CARTESIAN, selection);

// 设置目标力
RLWrench target_force = {0, 0, -20, 0, 0, 0};  // Z方向20N压力
RLPose task_frame = {0, 0, 0, 0, 0, 0};  // 基坐标系
controller->fcSetGoalForce(DescribeSpace::CARTESIAN, task_frame, target_force);

// 8. 创建规划器（位置规划+力控制）
auto planner_h = scene->rlCreateRobotPlanner({RobotRole{state_h, MASTER}}, "force_planner");
auto planner = scene->rlGetRobotPlanner(planner_h);

// 添加接触运动（先移动到接触点上方）
PathPoint approach_point, contact_point;
approach_point.describe_space = DescribeSpace::CARTESIAN;
approach_point.pose = {0.5, 0, 0.6, 0, 0, 0};  // 接触点上方100mm

contact_point.describe_space = DescribeSpace::CARTESIAN;
contact_point.pose = {0.5, 0, 0.5, 0, 0, 0};  // 接触点

planner->tpAddPositionLine(approach_point, path_prop, move_prop);
planner->tpAddPositionLine(contact_point, path_prop, move_prop);

// 9. 创建任务
auto task = scene->rlCreateRobotTask({planner_h}, "force_task");

// 10. 控制循环
while (true) {
    // 10.1 更新状态（包括力传感器）
    state->rsUpdateJointState(q, qd, qdd, torque, temp, friction);

    double ft_data[6];
    ft_sensor->read(ft_data);
    state->rsUpdateFTSensorData(FTSensorType::END_FT_SENSOR, ft_data);

    // 10.2 获取轨迹点
    std::vector<TrajectoryPoint> traj_points;
    PlannerHandle failed_h;
    int ret = task->tskUpdateCycle(0.005, traj_points, failed_h);

    // 10.3 计算控制指令（位置+力控制）
    JointCommand cmd;
    state->rsCalJointCommand(cmd);

    // 10.4 下发控制指令
    hardware->setJointCommand(cmd.position, cmd.velocity, cmd.acceleration, cmd.torque);

    // 10.5 监测力控状态
    RLWrench contact_force = state->rsGetEndExternalWrenchInEnd();
    if (std::abs(contact_force.force[2] + 20) < 1.0) {
        std::cout << "Target force reached!" << std::endl;
    }

    std::this_thread::sleep_for(std::chrono::milliseconds(5));
}

5. 设计优势总结

5.1 对比旧架构

5.2 SOLID原则体现

1. 单一职责原则（SRP）: ✅

   • Model: 只管参数
   • Solver: 只管计算
   • State: 只管状态
   • Planner: 只管规划
   • Task: 只管协调

2. 开闭原则（OCP）: ✅

   • 添加新的控制算法：扩展Controller，不修改其他模块
   • 添加新的规划算法：扩展Planner，不影响Task

3. 里氏替换原则（LSP）: ✅

   • 所有模块都是接口，可以有多种实现

4. 接口隔离原则（ISP）: ✅

   • 每个模块只暴露必要的接口
   • 用户只依赖所需的模块

5. 依赖倒置原则（DIP）: ✅

   • 高层（Task）依赖抽象（Planner接口）
   • 低层（具体实现）实现抽象

6. 最佳实践建议

6.1 初始化顺序

6.1.1 单机器人场景

1. Scene（场景）
   │
2. Model（机器人模型）
   │
3. State（状态管理器，依赖Model）
   │
4. Planner（规划器，依赖State）
   │
5. Task（任务管理器，依赖Planner）
   │
6. Controller（控制器，依赖State，可选）

代码示例：

// 1. 创建场景
auto scene = CreateARALScene("my_scene");

// 2. 创建机器人模型
auto model_h = scene->rlCreateRobotModel("aubo_i5", "/path/");
auto model = scene->rlGetRobotModel(model_h);

// 3. 创建状态管理器（Solver会自动创建）
auto state_h = scene->rlCreateRobotState(model_h, "real_state");
auto state = scene->rlGetRobotState(state_h);
auto solver = scene->rlGetRobotSolver(model_h);  // Solver可以随时获取

// 4. 创建规划器
auto planner_h = scene->rlCreateRobotPlanner(
    {RobotRole{state_h, PlannerRole::MASTER}}, "main_planner");
auto planner = scene->rlGetRobotPlanner(planner_h);

// 5. 创建任务（可选，单Planner也可以不创建Task）
auto task = scene->rlCreateRobotTask({planner_h}, "main_task");

// 6. 创建控制器（如果需要力控等高级功能）
auto controller = scene->rlCreateRobotController(state_h, "force_ctrl");

6.1.2 多机器人同步场景

1. Scene
   │
2. Model A, Model B（多个机器人模型）
   │
3. State A, State B（各机器人的状态管理器）
   │
4. Planner A, Planner B（各机器人的规划器）
   │
5. Task（统一管理多个Planner，实现同步）

代码示例：

// 1. 创建场景
auto scene = CreateARALScene("dual_arm_cell");

// 2. 创建两个机器人
auto model_A_h = scene->rlCreateRobotModel("robot_A", path_A);
auto model_B_h = scene->rlCreateRobotModel("robot_B", path_B);

// 3. 创建各自的状态
auto state_A_h = scene->rlCreateRobotState(model_A_h, "state_A");
auto state_B_h = scene->rlCreateRobotState(model_B_h, "state_B");

// 4. 创建各自的规划器
auto planner_A_h = scene->rlCreateRobotPlanner(
    {RobotRole{state_A_h, PlannerRole::MASTER}}, "planner_A");
auto planner_B_h = scene->rlCreateRobotPlanner(
    {RobotRole{state_B_h, PlannerRole::MASTER}}, "planner_B");

// 5. 创建同步任务
auto task_sync = scene->rlCreateRobotTask(
    {planner_A_h, planner_B_h}, "sync_task");

6.2 状态更新频率

• State更新: 200Hz（5ms）- 实时性要求高
• Planner规划: 200Hz（5ms）- 与控制周期一致
• Task采样: 200Hz（5ms）- 与控制周期一致
• Controller计算: 200Hz（5ms）- 与控制周期一致

6.3 错误处理

// 检查返回值
int ret = planner->tpAddPositionLine(target, path_prop, move_prop);
if (ret < 0) {
    auto utility = scene->rlGetUtility();
    std::string error_msg = utility->rlGetErrorDescription(ret);
    std::cerr << "Error: " << error_msg << std::endl;

    // 根据错误类型处理
    if (ret == E_IK_ANALYTICAL_SOLVER_FAILED) {
        // 目标点不可达
    } else if (ret == E_PLAN_OUT_OF_JOINT_LIMIT) {
        // 超出关节限制
    }
}

6.4 资源管理

// 使用智能指针，自动管理生命周期
{
    auto scene = CreateARALScene("temp");
    auto model_h = scene->rlCreateRobotModel("robot", path);
    // ...
} // scene 超出作用域，自动释放所有资源

6.5 线程安全

• ✅ Solver: 完全线程安全（无状态）
• ⚠️ State: 非线程安全（需要外部同步）
• ⚠️ Planner: 非线程安全（单线程使用）
• ⚠️ Task: 非线程安全（单线程使用）

// 多线程使用Solver（安全）
std::thread t1([&]{ solver->kdCalForwardPosition(...); });
std::thread t2([&]{ solver->kdCalInverseDynamics(...); });

// 多线程使用State（需要加锁）
std::mutex state_mutex;
std::thread t1([&]{ 
    std::lock_guard<std::mutex> lock(state_mutex);
    state->rsUpdateJointState(...); 
});

7. 迁移指南（从旧接口到新接口）

7.1 初始化对比

旧接口:

auto robot = CreateARALIntfacePtrFromFile("aubo_i5", "/path/");
robot->rlInitiateRobotModelFromFiles("aubo_i5", "/path/");

新接口:

auto scene = CreateARALScene("my_scene");
auto model_h = scene->rlCreateRobotModel("aubo_i5", "/path/");
auto model = scene->rlGetRobotModel(model_h);
auto solver = scene->rlGetRobotSolver(model_h);
auto state_h = scene->rlCreateRobotState(model_h, "state");
auto state = scene->rlGetRobotState(state_h);

7.2 运动学计算对比

旧接口:

robot->kdCalForwardPosition(q, T_f_e, false, T_b_e);

新接口:

solver->kdCalForwardPosition(q, T_f_e, false, T_b_e);

7.3 规划对比

旧接口:

robot->tpInitiatePlanner(q, qd, qdd, status);
robot->tpAddPositionLine(point, path_prop, move_prop, tool_workpiece);
robot->tpUpdateCycle(0.005, point);

新接口:

state->rsInitiateRobotState(q, qd, qdd, tool_workpiece);
auto planner_h = scene->rlCreateRobotPlanner({role}, "planner");
auto planner = scene->rlGetRobotPlanner(planner_h);
planner->tpAddPositionLine(point, path_prop, move_prop);

auto task = scene->rlCreateRobotTask({planner_h}, "task");
std::vector<TrajectoryPoint> points;
PlannerHandle failed;
task->tskUpdateCycle(0.005, points, failed);

8. 参考文档

• 多机协同规划方案
• robot_scene.hpp
• robot_model.hpp
• robot_solver.hpp
• robot_state.hpp
• robot_planner.hpp
• robot_task.hpp
• robot_utility.hpp